#include <new>
#include <memory>
#include <stdexcept>
#include <cmath>
#include <cstdlib>
#include <iostream>
#include <sstream>
#include <ctime>
#include <boost/shared_array.hpp>
#include <boost/random.hpp>
#include <boost/random/random_device.hpp>
#include <libplayerc++/playerc++.h>
#include "MapStateValidityChecker.h"
#include "SimpleExecutor.h"
#include "pause.h"
#include "ptRRT.h"
#include "FrictionModelImpl.h"
#include "VisualizerImpl.h"

using namespace PlayerCc;
using namespace ompl::base;
using namespace ompl::geometric;

class Controller {

  boost::random::random_device rng;
  boost::random::uniform_int_distribution<> coin;

  // player parameters
  PlayerClient    robot;
  SimulationProxy sim;
  MapProxy        map;
  Position2dProxy pp;
  Position2dProxy gocon;
  Graphics2dProxy g;

  // ompl parameters
  StateSpacePtr space;
  SimpleSetup ss;

  //
  boost::shared_ptr<Visualizer> visualizer;

public:

  Controller() :
    coin(0,1),
    robot("localhost"),
    sim(&robot, 0),
    map(&robot, 0),
    pp(&robot, 0),
    gocon(&robot, 100),
    g(&robot, 0),
    space(new SE2StateSpace()),
    ss(space),
    visualizer(new VisualizerImpl(g, pp))
  {
    robot.Read();
    map.RequestMap();

    // set bounds
    double xmax = map.GetWidth() * map.GetResolution() / 2.0;
    double ymax = map.GetHeight() * map.GetResolution() / 2.0;
    RealVectorBounds bounds(2);
    bounds.setLow(0, -xmax); bounds.setHigh(0, xmax);
    bounds.setLow(1, -ymax); bounds.setHigh(1, ymax);
    space->as<SE2StateSpace>()->setBounds(bounds);

    // set validity checker
    StateValidityCheckerPtr checker(new MapStateValidityChecker(0.4, ss.getSpaceInformation(), map));
    ss.setStateValidityChecker(checker);

  }

private:

  PlannerPtr getPlanner() {
    boost::shared_ptr<FrictionModel> friction(new FrictionModelImpl(sim));
    PlannerPtr planner(new ptRRT(ss.getSpaceInformation(), friction));
    planner->as<ptRRT>()->setRange(0.5);
    planner->as<ptRRT>()->setVisualizer(visualizer);
    return planner;
  }

  // plan motion from one start to one goal and execute
  void planAndExecute(const std::string & description, double goalX, double goalY) {
    // get current robot positions etc.
    robot.Read();

    // start at the existing position
    ScopedState<SE2StateSpace> start(ss.getSpaceInformation());
    start->setX(pp.GetXPos()); start->setY(pp.GetYPos());

    // goal is the standby position
    ScopedState<SE2StateSpace> goal(ss.getSpaceInformation());
    goal->setX(goalX); goal->setY(goalY);

    // visualize
    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ")" << std::endl;

    // set start and goal
    ss.setStartAndGoalStates(start, goal);

    // setup the path planner
    ss.setPlanner(getPlanner());
    ss.clear();
    ss.setup();

    // plan
    PlannerStatus solved = ss.solve();

    // execute
    if (solved) {
       util::pause(1); // show path
       visualizer->clear();
       SimpleExecutor executor(ss.getSolutionPath(), robot, pp, gocon);
       executor.execute();
       util::pause(1);
    }

    std::cout << description.c_str() << " (" << goalX << ", " << goalY << ") " << (solved? "done": "failed!") << std::endl;
    
  }

public:

  // control loop
  void run() {
    for (;;) {
      planAndExecute("Point", 2.0, 0.0);
      planAndExecute("Point", -4, 0.0);
    }
  }
};

int
main(int argc, char *argv[])
{
  try {
    Controller control;
    control.run();
    return 0;
  } catch (std::exception &e) {
    std::cerr << e.what() << std::endl;
    return 1;
  }
}
